#include <stdio.h>
#include <stdlib.h>
#include "task.h"
#include "port.h"
#include "kernel.h"
#include "heap.h"


TCB *TaskArray[MAX_TASKS];

#if HIGHEST_PRIO_ALT == 1
TCB *ReadyArray[MAX_TASKS];
#elif HIGHEST_PRIO_ALT == 2
uint_32 ReadyTaskBitmap[(MAX_TASKS >> 5) + 1];
uint_32 ReadyTaskIndex;
#elif HIGHEST_PRIO_ALT == HP_PORT_OPTIMAL
uint_32 ReadyTaskBitmap;
#endif
LIST_HEAD(allTasksLinked);

TCB *currentTCB;
TCB *highestTCB;
// it seems more convenient to to it like this:
/* uint_8 ReadyTaskBitmap[ (MAX_TASKS) >> 3 + 1];*/
/* Then also uint_8 ReadyTaskIndex.
   Then use a look up table of 256 for finding
   the first bit set. This is architecture independedn real time solution*/

err_t task_create(uint_8 prio, task function, void *args, uint_32 stk_size, uint_32 *tos)
{
	err_t ret = ERR_OK;
	TCB *newTCB;
	/* Check if task priority is bigger than the maximum allowed number of tasks
	   or if task priority is reserved by another task */
#if MULTIPLE_TASKS_PER_PRIORITY
	if ((prio > MAX_PRIO)){	
#else
	if ((prio > MAX_TASKS) || (TaskArray[prio] != NULL)){
#endif
		ret = ERR_INVALID_PRIO;
		return ret;
	}
	/* Initialize TCB and Stack */
 	newTCB = TCBAlloc();
	TCBInit(newTCB, prio, stk_size);
	/* StkInit is in port */
	newTCB->TopOfStkPtr = StkInit(tos, function, args, stk_size);
	/* register the task */
	TaskArray[newTCB->prio] = newTCB;
	
#if HIGHEST_PRIO_ALT == 1
	ReadyArray[newTCB->prio] = newTCB;
#elif HIGHEST_PRIO_ALT == 2	
	/* update ready task bitmap */
	ReadyTaskBitmap[(newTCB->prio >> 5)] |=  1 << (newTCB->prio & 0x1F);
	ReadyTaskIndex |= 1 << (newTCB->prio >> 5);
#elif HIGHEST_PRIO_ALT == 3
	ReadyTaskBitmap |= 1 << (newTCB->prio &0x1F);
#endif
	/*add to list of all linked tasks*/
	list_add_tail(&(newTCB->delay_list) , &allTasksLinked);  
	return ret;
}


TCB * TCBAlloc(void)
{
	/* depends on mem mang scheme*/
	#if MEM_MANG == 0
	TCB *tmpTCB = (TCB *)malloc(sizeof(TCB));
	//tmpTCB = (TCB *)malloc(sizeof(TCB));
	#endif
	return tmpTCB;
}

void TCBInit(TCB *newTCB, uint_8 prio, uint_32 stk_size)
{
	newTCB->prio = prio;
	newTCB->state = READY;
	newTCB->estate = NONE;
	newTCB->delay = 0;
	INIT_LIST_HEAD(&(newTCB->delay_list));
	INIT_LIST_HEAD(&(newTCB->event_list));
	//newTCB->StkPtr = (OSStackType *)malloc(stk_size * sizeof(OSStackType));
}

void tasksInit(void)
{
	int i;

	for (i = 0; i < MAX_TASKS; i++)
		TaskArray[i] = NULL;
	
#if HIGHEST_PRIO_ALT == 1
	
	for (i = 0; i < MAX_TASKS; i++)
		ReadyArray[i] = NULL;
	
#elif HIGHEST_PRIO_ALT == 2
	
	for (i = 0; i < (MAX_TASKS >> 5); i++)
		ReadyTaskBitmap[i] = 0;
	ReadyTaskIndex = 0;
	
#elif HIGHEST_PRIO_ALT == 3
	ReadyTaskBitmap = 0;
#endif
}

TCB *getTCBbyPrio(unsigned int prio)
{
	
	if (prio < MAX_TASKS)
		return TaskArray[prio];
	else 
		return NULL;
}

#if HIGHEST_PRIO_ALT == 1

void FindHighestPriorityTask()
{
	uint_32 highestPrio = MAX_TASKS - 1;
	while(ReadyArray[highestPrio] == NULL){
		highestPrio--;
	}
	highestTCB = ReadyArray[highestPrio];
}
//check lg_debruijn
#elif HIGHEST_PRIO_ALT == 2

/* or something like*/
uint_8 clz(uint_32 x)
{
	uint_8 n = 0;
    if (x == 0)
		return 32;
    if ((x & 0xFFFF0000) == 0){
		n = n + 16;
		x = x << 16;
	}
    if ((x & 0xFF000000) == 0){
		n = n + 8;
		x = x << 8;
	}
    if ((x & 0xF0000000) == 0){
		n = n + 4;
		x = x << 4;
	}
    if ((x & 0xC0000000) == 0){
		n = n + 2;
		x = x << 2;
	}
    if ((x & 0x80000000) == 0){
		n = n + 1;
	}		
    return n;
}

void FindHighestPriorityTask()
{
	highestTCB = TaskArray[31 - clz(ReadyTaskBitmap[31 - clz(ReadyTaskIndex)])];	
}
#endif
/* Better alternative? */

void schedule(void)
{
	ENTER_CRITICAL();
	FindHighestPriorityTask();
	if (currentTCB == highestTCB){
		EXIT_CRITICAL();
		return;
	}else{
		
		EXIT_CRITICAL();		
		SWITCH_CONTEXT();
	}
}

void yield()
{
	ENTER_CRITICAL();
	currentTCB->state = BLOCKED;
	
#if HIGHEST_PRIO_ALT == 1
	ReadyArray[currentTCB->prio] = NULL;
#elif HIGHEST_PRIO_ALT == 2
	ReadyTaskBitmap[(currentTCB->prio >> 5)] &=  ~(1 << (currentTCB->prio & 0x1F)) ;	
	ReadyTaskIndex &= ~(1 << (currentTCB->prio >> 5));
#elif HIGHEST_PRIO_ALT == 3
	ReadyTaskBitmap &=  ~(1 << (currentTCB->prio & 0x1F));	
#endif
	EXIT_CRITICAL();
	schedule();
}


err_t TaskEnable(uint_16 prio)
{
	ENTER_CRITICAL();
	if (TaskArray[prio] == NULL){
		EXIT_CRITICAL();
		return 	ERR_INVALID_PRIO;
	}
	TaskArray[prio]->state = READY;
	

	#if HIGHEST_PRIO_ALT == 1
	ReadyArray[prio] = TaskArray[prio];
	#elif HIGHEST_PRIO_ALT == 2
	ReadyTaskBitmap[(prio >> 5)] |= 1 << (prio & 0x1F);
	ReadyTaskIndex |= 1 << (prio >> 5);
	#elif HIGHEST_PRIO_ALT == 3
	ReadyTaskBitmap |= 1 << (prio & 0x1F);
	#endif
	EXIT_CRITICAL();
	schedule();
	return ERR_OK;
}

err_t TaskDisable(uint_16 prio)
{
	if (TaskArray[prio] == NULL)
		return ERR_INVALID_PRIO;
	if (prio == currentTCB->prio){
		yield();
		return ERR_OK;
	}else{
		ENTER_CRITICAL();
		TaskArray[prio]->state = BLOCKED;
		
#if HIGHEST_PRIO_ALT == 1 
		ReadyArray[prio] = NULL;
#elif HIGHSET_PRIO_ALT == 2
		ReadyTaskBitmap[ (prio >>5)] &= ~(1 << (prio & 0x1F));
		ReadyTaskIndex &= ~(1 << (prio >> 5));
#elif HIGHEST_PRIO_ALT == 3
		ReadyTaskBitmap &= ~(1 << (prio & 0x1F));		
#endif
		EXIT_CRITICAL();
		return ERR_OK;
	}
}


